#!/usr/bin/env python3

import rospy
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image

def main():
    # 初始化 ROS 节点
    rospy.init_node('image_publisher', anonymous=True)

    # 创建 VideoCapture 对象
    cap = cv2.VideoCapture(0)
    bridge = CvBridge()

    # 创建图像发布者
    image_pub = rospy.Publisher('camera/image_raw', Image, queue_size=10)

    # 设置循环频率
    rate = rospy.Rate(10)  # 10 Hz

    while not rospy.is_shutdown():
        # 捕捉图像帧
        ret, frame = cap.read()
        if not ret:
            rospy.logerr("Failed to capture image")
            continue

        # 将 OpenCV 图像转换为 ROS 图像消息
        try:
            ros_image = bridge.cv2_to_imgmsg(frame, encoding='bgr8')
        except Exception as e:
            rospy.logerr(f"Error converting image: {e}")
            continue

        # 发布图像消息
        image_pub.publish(ros_image)

        # 按照设定频率休眠
        rate.sleep()

    # 释放摄像头资源
    cap.release()

if __name__ == '__main__':
    main()

